from machine import ADC,Pin,freq,PWM
from setcfg import DAT
#湃湃狗扬声器引脚 13，其他的自己查一下
beep = PWM(Pin(DAT['beep']),freq=1, duty=0)
beep_count=0
beep_freq=0
beep_duty=0

def beep_control(freq=0, duty=0):
    global beep_count,beep_freq,beep_duty
    """
    控制报警器的函数。
    :param freq: 报警频率（Hz），设置为0时停止报警
    :param duty: PWM 占空比（0-1023，具体范围取决于硬件）
    """
    if freq == 0:
        beep.freq(1)
        beep.duty(0)
        beep_count=0
        beep_freq=0
        beep_duty=0
    else:
        beep_freq = freq
        beep_duty=duty
        

def beep_run():
    global beep_count,beep_freq,beep_duty
   # print("beep %d %d %d"%(beep_freq,beep_duty))
    if beep_freq==0:
        return    
    beep_count =(beep_count + 1)%20
    if beep_count==0:
        beep.freq(beep_freq)
        beep.duty(beep_duty)
    else:
        #beep.freq(1)
        beep.duty(0)

def read_voltage(x):   #电压拟合
  p1 = 0.002575
  p2 = 0.7446
  return p1*x + p2

#adc采样引脚，湃湃狗
adc = ADC(Pin(DAT['adc']))
empty_power_count=0
IK_ERROR=0;error_node=0;normal_node=0
def alarm_and_servo_control():
  global empty_power_count,IK_ERROR,error_node,normal_node,adcReadCount
  try:
    judge_num_node=0
    adc_v=read_voltage(adc.read())
  except Exception as e:
    print("adc exception "+str(e))
    adc_v = 0  # Initialize adc_v in case of exception
    judge_num_node = 0  # Initialize judge_num_node in case of exception


  #Battary judge
  if (adc_v<5.1 and adc_v>0.8) or adc_v>6.1:  
    empty_power_count=empty_power_count+1
    print(f"low V={adc_v}")
  else:
    empty_power_count=0
  if empty_power_count>=33:
    judge_num_node=judge_num_node+1
    empty_power_count=33   
  
  if adc_v<0.8:   #接入USB时
    judge_num_node=judge_num_node+1
  if IK_ERROR==1:
    judge_num_node=judge_num_node+1
  #IK_ERROR ALARM
  if IK_ERROR==1 and error_node==0:    
    error_node=1
    normal_node=0
    print("逆解算错误，检查运动是否超限，为保护舵机，舵机电源已急停")
  if IK_ERROR==0 and normal_node==0:   #Focus on Clear to Zero
    error_node=0
    normal_node=1
  #print("%f  %d  %d"%(adc_v,empty_power_count,judge_num_node))
  return judge_num_node

import wifi_moudle
logtoudp = wifi_moudle.LogToUdp(8765)

